﻿#define UNSCALED_TIME
using Devil;
using GameToolkit;
using System.Collections.Generic;
using Unity.Burst;
using Unity.Collections;
using Unity.Jobs;
using UnityEngine;
using UnityEngine.Jobs;
using UnityEngine.Profiling;

namespace DynamicBone
{
    //[AddComponentMenu("")]
    [DefaultExecutionOrder(1000), DisallowMultipleComponent]
    public class DynamicBoneJobs : MonoBehaviour //, IPreCullAction
    {
        static NativeDynamicBoneRoot.NativePool sNativePool;
        static bool sManualUpdate;
        static ulong sManualUpdateID;

        public static void AutoUpdate()
        {
            if (sManualUpdate)
            {
                sManualUpdate = false;
                sManualUpdateID = 0;
            }
        }

        public static void ManualUpdate(ulong id, float time, float deltaTime, float unscaledDeltaTime)
        {
            if (sNativePool != null && sManualUpdateID != id)
            {
                sManualUpdateID = id;
                if (!sManualUpdate)
                {
                    sManualUpdate = true;
                    sNativePool.FlushJobs();
                }
                if (sActive != null)
                {
                    sNativePool.Schedule(sActive.m_MaxMoveDistance, sActive.m_GravityMultipier, sActive.m_WindMultipier, sActive.m_WindArea,
                        time, deltaTime, unscaledDeltaTime);
                    sNativePool.FlushJobs();
                }
            }
        }

#if UNITY_EDITOR
        private static void ChangePlayMode(UnityEditor.PlayModeStateChange obj)
        {
            if ( obj == UnityEditor.PlayModeStateChange.ExitingPlayMode)
            {
                if (sNativePool != null)
                {
                    sNativePool.Dispose();
                    sNativePool = null;
                }
                sActives.Clear();
                sActive = null;
            }
        }
#endif

        public static NativeDynamicBoneRoot.NativePool GetNativePool()
        {
            if (sNativePool == null)
            {
                sNativePool = new NativeDynamicBoneRoot.NativePool();
#if UNITY_EDITOR
                UnityEditor.EditorApplication.playModeStateChanged += ChangePlayMode;
#endif
            }
            return sNativePool;
        }

        public static NativeDynamicBoneRoot AddRootBone(DynamicBoneRoot system)
        {
            var nativePool = GetNativePool();
            var root = nativePool.AddRootBone(system);
            if (sActives.Count == 0)
            {
                var go = new GameObject("[Default Dynamic Bone Jobs]");
                var com = go.AddComponent<DynamicBoneJobs>();
                com.mActivePriority = com.CalculatePriority();
                if (!sActives.Contains(com))
                {
                    sActives.Add(com);
                    sActive = com;
                }
            }
            return root;
        }

        public static void RemoveRootBone(NativeDynamicBoneRoot inst)
        {
            if (sNativePool != null)
            {
                sNativePool.RemoveRootBone(inst);
            }
        }

        public static bool AddUpdateData(NativeDynamicBoneRoot inst)
        {
            if (sNativePool != null)
            {
                sNativePool.AddUpdateData(inst);
                return true;
            }
            return false;
        }

        public static void RemoveUpdateData(NativeDynamicBoneRoot inst)
        {
            if (sNativePool != null)
            {
                sNativePool.RemoveUpdateData(inst);
            }
        }

        public static void MarkRootDirty(NativeDynamicBoneRoot inst, bool fullUpdate = false)
        {
            if (sNativePool != null)
            {
                sNativePool.MarkRootDirty(inst, fullUpdate);
            }
        }

        public static NativeInstance<DynamicCollider> AddCollider(DynamicBoneColliderBase collider)
        {
            if (sNativePool != null)
            {
                return sNativePool.AddCollider(collider);
            }
            else
            {
                return null;
            }
        }

        public static void RemoveCollider(NativeInstance<DynamicCollider> inst)
        {
            if (sNativePool != null)
            {
                sNativePool.RemoveCollider(inst);
            }
        }

        static DynamicBoneJobs sActive;
        static MinHeap<DynamicBoneJobs> sActives = new MinHeap<DynamicBoneJobs>(8, (a, b) => b.mActivePriority - a.mActivePriority, (a, b) => a == b);
        [SerializeField, Range(0, 100), Tooltip("应用优先级")]
        int m_Priority;
        
        [Range(-1, 1)]
        public float m_GravityMultipier = 0f;
        [Range(0, 1)]
        public float m_WindMultipier = 0f;
        [Tooltip("允许每次模拟移动的最大距离")]
        public float m_MaxMoveDistance = 2f;

        public WindZone m_WindArea;

        //[SerializeField]
        bool m_IsActiveJobs;
        int mActivePriority;
        //float mWeight;

        int CalculatePriority()
        {
            return (m_Priority << 8) + (sActives.Count & 0xff);
        }

        private void OnEnable()
        {
            if (!sActives.Contains(this))
            {
                mActivePriority = CalculatePriority();
                sActives.Add(this);
                sActive = sActives.TopValule;
                //if (sActive != null)
                //    CullingManager.Instance.RemovePreCullAction(sActive);
                //sActive = this;
                //CullingManager.Instance.AddPreCullAction(this);
            }
        }

        private void OnDisable()
        {
            m_IsActiveJobs = false;
            sActives.Remove(this);
            sActive = sActives.Count > 0 ? sActives.TopValule : null;
            //var last = sActives.Count > 0 ? sActives[sActives.Count - 1] : null;
            //if (last != sActive)
            //{
            //    if (sActive != null)
            //        CullingManager.Instance.RemovePreCullAction(sActive);
            //    sActive = last;
            //    if (sActive != null)
            //        CullingManager.Instance.AddPreCullAction(sActive);
            //}
        }

        private void OnDestroy()
        {
            if (sActive == null && sNativePool != null)
            {
                sNativePool.Dispose();
                sNativePool = null;
            }
        }

        private void Update()
        {
            m_IsActiveJobs = sActive == this;
            if (sNativePool != null && m_IsActiveJobs && !sManualUpdate)
            {
                sNativePool.Schedule(m_MaxMoveDistance, m_GravityMultipier, m_WindMultipier, m_WindArea, Time.time, Time.deltaTime, Time.unscaledDeltaTime);
                //sNativePool.FlushJobs();
            }
        }

        private void LateUpdate()
        {
            if (sNativePool != null && m_IsActiveJobs && !sManualUpdate)
            {
                sNativePool.FlushJobs();
            }
        }
        

#if UNITY_EDITOR
        private void OnValidate()
        {
            if(this != null && isActiveAndEnabled)
            {
                var priority = CalculatePriority();
                if(priority != mActivePriority)
                {
                    sActives.Remove(this);
                    mActivePriority = priority;
                    sActives.Add(this);
                    sActive = sActives.TopValule;
                }
            }
        }
#endif
    }

    public enum EDynamicParticleFlags : byte
    {
        //// 骨骼末端
        //IsEndOffset = 0x1,
        // 被碰撞
        IsCollide = 0x2,
        //// 重置系统时间
        //ResetDynamicTime = 0x4,

        // 无碰撞
        NoCollide = 0x7d,

        FixedOrientation = 0x8,
    }

    public enum EDynamicBoneFlags : byte
    {
        FreseAxisX = 0x1,
        FreseAxisY = 0x2,
        FreseAxisZ = 0x4,

        UnscaledTime = 0x10,

        // 本地模拟
        LocalSimulate = 0x20,

        FreseAxisMask = 0x7,
    }

    public struct DynamicParticle
    {
        // How much the bones slowed down
        public short parentIndex;
        public float damping;
        // How much the force applied to return each bone to original orientation
        public float elasticity;
        // How much bone's original orientation are preserved
        public float stiffness;
        // How much character's position change is ignored in physics simulation
        public float inert;
        // How much the bones slowed down when collide
        public float friction;
        // Each bone can be a sphere to collide with colliders. Radius describe sphere's size
        public float radius;
        public Vector3 initLocalPosition;
        public Quaternion initLocalRotation;

        public EDynamicParticleFlags flags;
        public Vector3 position;
        public Quaternion rotation;
        public Vector3 prevPosition;
    }

    public enum EDynamicColliderType : byte
    {
        Plane,
        OutsideSphere,
        InsideSphere,
        OutsideCapsule,
        InsideCapsule,
    }

    public struct DynamicCollider
    {
        public int id;
        public EDynamicColliderType type;
        public Vector3 center;
        public Vector3 size;
        public Vector3 position;
        public Quaternion rotation;
    }

    public struct DynamicColliderID
    {
        public int id;
        public short index;
    }

    public struct DynamicBoneRootData
    {
        public byte layer;
        public float updateRatio;
        public Quaternion orientation;
        public Vector3 force;
        public short particleIndex;
        public short particleCount;

        public short colliderIndex;
        public short colliderCount;

        public Vector3 prevPosition;
        public float radius;

        public float forceWeight;
        public EDynamicBoneFlags flags;
        public float time;
    }

#if BURST_COMPILE
    [BurstCompile]
#endif
    public struct UpdateDynamicColliderJob : IJobParallelForTransform
    {
        [NativeDisableParallelForRestriction]
        public NativeArray<DynamicCollider> colliders;
        //public NativeArray<short> accessIndex;
        public void Execute(int index, TransformAccess transform)
        {
            //var i = accessIndex[index];
            //var mat = Matrix4x4.TRS(transform.position, transform.rotation, transform.localScale);
            var col = colliders[index];
            var pos = transform.position + transform.rotation * col.center;
            col.position = pos; // = Matrix4x4.TRS(transform.position, transform.rotation, transform.localScale);
            col.rotation = transform.rotation;
            colliders[index] = col;
        }
    }

#if BURST_COMPILE
    [BurstCompile]
#endif
    public struct FindDynamicColliderJob : IJobParallelFor
    {
        public int colliderCount;
        [ReadOnly]
        public NativeArray<DynamicCollider> colliders;
        [ReadOnly]
        public NativeArray<DynamicBoneRootData> dynamicSystems;
        [ReadOnly]
        public NativeArray<short> accessIndex;
        [NativeDisableParallelForRestriction]
        public NativeArray<DynamicColliderID> colliderIds;

        public void Execute(int index)
        {
            var n = accessIndex[index];
            var sys = dynamicSystems[n];
            for (int i = 0; i < sys.colliderCount; i++)
            {
                var id = colliderIds[i + sys.colliderIndex];
                id.index = -1;
                for (int j = 0; j < colliderCount; j++)
                {
                    if (id.id == colliders[j].id)
                    {
                        id.index = (short)j;
                        break;
                    }
                }
                colliderIds[i + sys.colliderIndex] = id;
            }
        }
    }

#if BURST_COMPILE
    [BurstCompile]
#endif
    public struct UpdateDynamicBoneJob : IJobParallelForTransform
    {
        [ReadOnly]
        public NativeArray<short> accessIndex;

        [NativeDisableParallelForRestriction]
        public NativeArray<DynamicBoneRootData> dynamicSystems;

        [NativeDisableParallelForRestriction]
        public NativeArray<DynamicParticle> particles;

        [ReadOnly]
        public NativeArray<DynamicCollider> colliders;
        [ReadOnly]
        public NativeArray<DynamicColliderID> colliderIds;

        public float randomSeed;
        public float deltaTime;
        public float unscaledDeltaTime;
        public Vector3 gravity;
        public Vector3 windForce;
        public float windFrequency;
        public float maxDistance;

        public void Execute(int index, TransformAccess transform)
        {
            index = accessIndex[index];
            var sys = dynamicSystems[index];
            var pos = transform.position;
            var objMove = pos - sys.prevPosition;
            if ((sys.flags & EDynamicBoneFlags.LocalSimulate) != 0)
            {
                for (int i = 0; i < sys.particleCount; i++)
                {
                    var p = particles[i + sys.particleIndex];
                    p.prevPosition += objMove;
                    p.position += objMove;
                    particles[i + sys.particleIndex] = p;
                }
                objMove = Vector3.zero;
            }
            else
            {
                var dis = objMove.magnitude;
                if(dis > maxDistance)
                {
                    var normal = objMove / dis;
                    var delta = normal * (dis - maxDistance);
                    for (int i = 0; i < sys.particleCount; i++)
                    {
                        var p = particles[i + sys.particleIndex];
                        p.prevPosition += delta;
                        p.position += delta;
                        particles[i + sys.particleIndex] = p;
                    }
                    objMove = normal * maxDistance;
                }
            }
            sys.prevPosition = pos;
            int loop = 1;
            //float gscale = transform.localScale.x;
            if (sys.updateRatio > 0)
            {
                var dtime = (sys.flags & EDynamicBoneFlags.UnscaledTime) == 0 ? deltaTime : unscaledDeltaTime;
                //float dt = 1.0f / sys.updateRatio;
                sys.time += dtime;
                loop = Mathf.FloorToInt(sys.time / sys.updateRatio);
                if (loop >= 3)
                {
                    loop = 3;
                    sys.time = 0;
                }
                else
                {
                    sys.time -= loop * sys.updateRatio;
                }
            }

            dynamicSystems[index] = sys;

            Matrix4x4 toWorld = transform.localToWorldMatrix;// Matrix4x4.TRS(transform.position, transform.rotation, transform.localScale);
            if (loop > 0)
            {
                for (int i = 0; i < loop; ++i)
                {
                    UpdateParticles1(sys, toWorld, objMove);
                    UpdateParticles2(sys, toWorld);
                    objMove = Vector3.zero;
                }
            }
            else
            {
                SkipUpdateParticles(sys, toWorld, objMove);
            }
            ApplyParticlesRatation(sys, transform.rotation);
        }

        void UpdateParticles1(DynamicBoneRootData sys, Matrix4x4 toWorld, Vector3 objMove)
        {
            Vector3 force = windForce * Mathf.PerlinNoise((sys.prevPosition.x + randomSeed) * windFrequency, sys.prevPosition.y * windFrequency) * sys.forceWeight;
            force += gravity * sys.forceWeight;
            //Vector3 fdir = force.normalized;
            //Vector3 rf = sys.orientation * gravity; // m_Root.TransformDirection(m_LocalGravity);
            //Vector3 pf = fdir * Mathf.Max(Vector3.Dot(rf, fdir), 0);	// project current gravity to rest gravity
            //force -= pf;	// remove projected gravity
            force += sys.force;

            for (int i = 0; i < sys.particleCount; ++i)
            {
                var p = particles[i + sys.particleIndex];
                if (p.parentIndex >= 0)
                {
                    // verlet integration
                    Vector3 v = p.position - p.prevPosition;
                    Vector3 rmove = objMove * p.inert;
                    p.prevPosition = p.position + rmove;
                    float damping = p.damping;
                    if ((p.flags & EDynamicParticleFlags.IsCollide) != 0)
                    {
                        damping += p.friction;
                        if (damping > 1)
                            damping = 1;
                        p.flags &= EDynamicParticleFlags.NoCollide;
                    }
                    p.position += v * (1 - damping) + force + rmove;
                }
                else
                {
                    p.prevPosition = p.position;
                    p.position = toWorld.MultiplyPoint(p.initLocalPosition);// p.m_Transform.position;
                }
                particles[i + sys.particleIndex] = p;
            }
        }

        void UpdateParticles2(DynamicBoneRootData sys, Matrix4x4 toWorld)
        {
            for (int i = 0; i < sys.particleCount; ++i)
            {
                var p = particles[i + sys.particleIndex];
                if (p.parentIndex == -1)
                    continue;
                var p0 = particles[p.parentIndex + sys.particleIndex];
                float restLen = Vector3.Distance(toWorld.MultiplyPoint(p0.initLocalPosition), toWorld.MultiplyPoint(p.initLocalPosition));
                // keep shape
                float stiffness = Mathf.Lerp(1.0f, p.stiffness, sys.forceWeight);
                Matrix4x4 localMatrix0 = toWorld * Matrix4x4.TRS(p0.initLocalPosition, p0.initLocalRotation, Vector3.one);
                if (stiffness > 0 || p.elasticity > 0)
                {
                    Matrix4x4 m0 = localMatrix0; // p0.m_Transform.localToWorldMatrix;
                    m0.SetColumn(3, p0.position);
                    var rot = p0.initLocalRotation;
                    rot.x = -rot.x;
                    rot.y = -rot.y;
                    rot.z = -rot.z;
                    // ??????
                    Vector3 restPos = m0.MultiplyPoint3x4(rot * (p.initLocalPosition - p0.initLocalPosition));

                    Vector3 d = restPos - p.position;
                    p.position += d * p.elasticity;

                    if (stiffness > 0)
                    {
                        d = restPos - p.position;
                        float len = d.magnitude;
                        float maxlen = restLen * (1 - stiffness) * 2;
                        if (len > maxlen)
                            p.position += d * ((len - maxlen) / len);
                    }
                }

                // collide
                if (sys.colliderCount > 0)
                {
                    RaycastCollider(sys, ref p);
                }

                // freeze axis, project to plane 
                var frese = sys.flags & EDynamicBoneFlags.FreseAxisMask;
                if (frese != 0)
                {
                    var normal = frese == EDynamicBoneFlags.FreseAxisX ? Vector3.right : (frese == EDynamicBoneFlags.FreseAxisY ? Vector3.up : Vector3.forward);
                    normal = localMatrix0.MultiplyVector(normal);
                    p.position -= Vector3.Dot(normal, p.position - p0.position) * normal;
                }

                // keep length
                Vector3 dd = p0.position - p.position;
                float leng = dd.magnitude;
                if (leng > 0)
                    p.position += dd * ((leng - restLen) / leng);
                particles[i + sys.particleIndex] = p;
            }
        }

        void RaycastCollider(DynamicBoneRootData sys, ref DynamicParticle particle)
        {
            for (int i = 0; i < sys.colliderCount; i++)
            {
                var id = colliderIds[i + sys.colliderIndex];
                if (id.index == -1)
                    continue;
                var col = colliders[id.index];
                switch (col.type)
                {
                    case EDynamicColliderType.InsideSphere:
                        RaycastInsideSphere(col, sys, ref particle);
                        break;
                    case EDynamicColliderType.OutsideSphere:
                        RaycastOutsideSphere(col, sys, ref particle);
                        break;
                    case EDynamicColliderType.InsideCapsule:
                        RaycastInsideCapsule(col, sys, ref particle);
                        break;
                    case EDynamicColliderType.OutsideCapsule:
                        RaycastOutsideCapsule(col, sys, ref particle);
                        break;
                    default:
                        break;
                }
            }
        }

        void RaycastInsideSphere(DynamicCollider col, DynamicBoneRootData sys, ref DynamicParticle particle)
        {
            var r = col.size.x * 0.5f - particle.radius;
            var r2 = r * r;
            var dir = particle.position - col.position;
            var len2 = dir.sqrMagnitude;

            // if is outside sphere, project onto sphere surface
            if (len2 > r2)
            {
                float len = Mathf.Sqrt(len2);
                particle.position = col.position + dir * (r / len);
                particle.flags |= EDynamicParticleFlags.IsCollide;
            }
        }

        void RaycastOutsideSphere(DynamicCollider col, DynamicBoneRootData sys, ref DynamicParticle particle)
        {
            var r = col.size.x * 0.5f + particle.radius;
            var r2 = r * r;
            var dir = particle.position - col.position;
            var len2 = dir.sqrMagnitude;

            // if is inside sphere, project onto sphere surface
            if (len2 > 0 && len2 < r2)
            {
                float len = Mathf.Sqrt(len2);
                particle.position = col.position + dir * (r / len);
                particle.flags |= EDynamicParticleFlags.IsCollide;
            }
        }

        void RaycastInsideCapsule(DynamicCollider col, DynamicBoneRootData sys, ref DynamicParticle particle)
        {
            float capRadius;
            Vector3 dir, c0, c1;
            if (col.size.x > col.size.y && col.size.x > col.size.z)
            {
                capRadius = col.size.y < col.size.z ? col.size.y : col.size.z;
                dir = col.rotation * Vector3.right * (col.size.x - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            else if (col.size.y > col.size.x && col.size.y > col.size.z)
            {
                capRadius = col.size.x < col.size.z ? col.size.x : col.size.z;
                dir = col.rotation * Vector3.up * (col.size.y - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            else
            {
                capRadius = col.size.x < col.size.y ? col.size.x : col.size.y;
                dir = col.rotation * Vector3.forward * (col.size.z - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            var r = capRadius - particle.radius;
            var r2 = r * r;
            Vector3 d = particle.position - c0;
            float t = Vector3.Dot(d, dir);

            if (t <= 0)
            {
                // check sphere1
                var len2 = d.sqrMagnitude;
                if (len2 > r2)
                {
                    float len = Mathf.Sqrt(len2);
                    particle.position = c0 + d * (r / len);
                    particle.flags |= EDynamicParticleFlags.IsCollide;
                }
            }
            else
            {
                float dl = dir.sqrMagnitude;
                if (t >= dl)
                {
                    // check sphere2
                    d = particle.position - c0;
                    var len2 = d.sqrMagnitude;
                    if (len2 > r2)
                    {
                        float len = Mathf.Sqrt(len2);
                        particle.position = c1 + d * (r / len);
                        particle.flags |= EDynamicParticleFlags.IsCollide;
                    }
                }
                else if (dl > 0)
                {
                    // check cylinder
                    t /= dl;
                    d -= dir * t;
                    var len2 = d.sqrMagnitude;
                    if (len2 > r2)
                    {
                        float len = Mathf.Sqrt(len2);
                        particle.position += d * ((r - len) / len);
                        particle.flags |= EDynamicParticleFlags.IsCollide;
                    }
                }
            }
        }


        void RaycastOutsideCapsule(DynamicCollider col, DynamicBoneRootData sys, ref DynamicParticle particle)
        {
            float capRadius;
            Vector3 dir, c0, c1;
            if (col.size.x > col.size.y && col.size.x > col.size.z)
            {
                capRadius = col.size.y < col.size.z ? col.size.y : col.size.z;
                dir = col.rotation * Vector3.right * (col.size.x - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            else if (col.size.y > col.size.x && col.size.y > col.size.z)
            {
                capRadius = col.size.x < col.size.z ? col.size.x : col.size.z;
                dir = col.rotation * Vector3.up * (col.size.y - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            else
            {
                capRadius = col.size.x < col.size.y ? col.size.x : col.size.y;
                dir = col.rotation * Vector3.forward * (col.size.z - capRadius);
                capRadius *= 0.5f;
                c0 = col.position - dir * 0.5f;
                c1 = c0 + dir;
            }
            float r = capRadius + particle.radius;
            float r2 = r * r;
            Vector3 d = particle.position - c0;
            float t = Vector3.Dot(d, dir);

            if (t <= 0)
            {
                // check sphere1
                float len2 = d.sqrMagnitude;
                if (len2 > 0 && len2 < r2)
                {
                    float len = Mathf.Sqrt(len2);
                    particle.position = c0 + d * (r / len);
                    particle.flags |= EDynamicParticleFlags.IsCollide;
                }
            }
            else
            {
                float dl = dir.sqrMagnitude;
                if (t >= dl)
                {
                    // check sphere2
                    d = particle.position - c1;
                    float len2 = d.sqrMagnitude;
                    if (len2 > 0 && len2 < r2)
                    {
                        float len = Mathf.Sqrt(len2);
                        particle.position = c1 + d * (r / len);
                        particle.flags |= EDynamicParticleFlags.IsCollide;
                    }
                }
                else if (dl > 0)
                {
                    // check cylinder
                    t /= dl;
                    d -= dir * t;
                    float len2 = d.sqrMagnitude;
                    if (len2 > 0 && len2 < r2)
                    {
                        float len = Mathf.Sqrt(len2);
                        particle.position += d * ((r - len) / len);
                        particle.flags |= EDynamicParticleFlags.IsCollide;
                    }
                }
            }
        }


        // only update stiffness and keep bone length
        void SkipUpdateParticles(DynamicBoneRootData sys, Matrix4x4 toWorld, Vector3 objMove)
        {
            for (int i = 0; i < sys.particleCount; ++i)
            {
                var p = particles[i + sys.particleIndex];
                if (p.parentIndex >= 0)
                {
                    p.prevPosition += objMove;
                    p.position += objMove;

                    var p0 = particles[p.parentIndex + sys.particleIndex];

                    float restLen;
                    //var isEndoff = (p.flags & (byte)EParticleFlags.IsEndOffset) != 0;
                    //if (isEndoff)
                    //    restLen = toWorld.MultiplyVector(p.initLocalPosition).magnitude;
                    //else
                    restLen = Vector3.Distance(toWorld.MultiplyPoint(p0.initLocalPosition), toWorld.MultiplyPoint(p.initLocalPosition));

                    // keep shape
                    float stiffness = Mathf.Lerp(1.0f, p.stiffness, sys.forceWeight);
                    if (stiffness > 0)
                    {
                        Matrix4x4 m0 = toWorld * Matrix4x4.TRS(p0.initLocalPosition, p0.initLocalRotation, Vector3.one);
                        m0.SetColumn(3, p0.position);
                        Vector3 restPos = m0.MultiplyPoint3x4(p0.initLocalRotation * (p.initLocalPosition - p0.initLocalPosition));

                        Vector3 d = restPos - p.position;
                        float len = d.magnitude;
                        float maxlen = restLen * (1 - stiffness) * 2;
                        if (len > maxlen)
                            p.position += d * ((len - maxlen) / len);
                    }

                    // keep length
                    Vector3 dd = p0.position - p.position;
                    float leng = dd.magnitude;
                    if (leng > 0)
                        p.position += dd * ((leng - restLen) / leng);
                }
                else
                {
                    p.prevPosition = p.position;
                    p.position = toWorld.MultiplyPoint(p.initLocalPosition);// p.m_Transform.position;
                }
                particles[i + sys.particleIndex] = p;
            }

        }

        void ApplyParticlesRatation(DynamicBoneRootData sys, Quaternion sysRotation)
        {
            //bool fix = (sys.flags & (byte)EDynamicBoneFlags.FixedOrientation) != 0;
            for (int i = 0; i < sys.particleCount; ++i)
            {
                var p = particles[i + sys.particleIndex];
                if (p.parentIndex == -1)
                    continue;
                var p0 = particles[p.parentIndex + sys.particleIndex];
                if ((p0.flags & EDynamicParticleFlags.FixedOrientation) == 0)
                {
                    Vector3 v = sysRotation * (p.initLocalPosition - p0.initLocalPosition);
                    Vector3 v2 = p.position - p0.position;
                    Quaternion rot = Quaternion.FromToRotation(v, v2);
                    p0.rotation = sysRotation * rot * p0.initLocalRotation;
                    particles[p.parentIndex + sys.particleIndex] = p0;
                }
            }
        }

    }

#if BURST_COMPILE
    [BurstCompile]
#endif
    public struct ApplyDynamicBoneJob : IJobParallelForTransform
    {
        [ReadOnly]
        public NativeArray<short> accessIndex;
        //[ReadOnly]
        //public NativeArray<DynamicBoneSystemData> dynamicSystem;
        [ReadOnly]
        public NativeArray<DynamicParticle> particles;

        public void Execute(int index, TransformAccess transform)
        {
            var p = particles[accessIndex[index]];
            transform.position = p.position;
            transform.rotation = p.rotation;
        }
    }

    public class NativeDynamicBoneRoot
    {
        int mNativeId;
        NativePool mPool;
        DynamicBoneRoot mSystem;
        Transform[] mParticleTransforms;
        DynamicBoneColliderBase[] mColliders;

        public bool IsAllocated { get { return mPool != null && mNativeId != -1; } }

        public NativeDynamicBoneRoot(DynamicBoneRoot system)
        {
            this.mSystem = system;
            mNativeId = -1;
        }

        public DynamicBoneRootData SystemData
        {
            get
            {
                mPool.FlushJobs();
                return mNativeId == -1 ? default : mPool.GetSystem(mNativeId);
            }
        }

        public int ParticleCount
        {
            get
            {
                var sys = SystemData;
                return sys.particleCount;
            }
        }

        public DynamicParticle GetParticle(int index)
        {
            return mPool.GetParticle(this, index);
        }

        public int ColliderCount
        {
            get
            {
                var sys = SystemData;
                return sys.colliderCount;
            }
        }

        public DynamicCollider GetCollider(int index)
        {
            return mPool.GetCollider(this, index);
        }

        public void Reset()
        {
            if (mPool == null || mNativeId == -1)
                return;
            var sys = SystemData;
            var toworld = mSystem.transform.localToWorldMatrix;
            var rot = mSystem.transform.rotation;
            for (int i = 0; i < sys.particleCount; i++)
            {
                var trans = mParticleTransforms[i];
                if (trans != null)
                {
                    var p = mPool.GetParticle(this, i);
                    trans.position = toworld.MultiplyPoint(p.initLocalPosition);
                    trans.rotation = rot * p.initLocalRotation;
                }
            }
        }


        public class NativePool : System.IDisposable
        {
            List<NativeDynamicBoneRoot> mSystemInstances;
            NativeArray<DynamicBoneRootData> mSystems;
            int mParticleCount;
            NativeArray<DynamicParticle> mParticles;
            NativeArray<DynamicColliderID> mColliderIDs;
            int mColliderIdCount;

            NativeInstance<DynamicCollider>.Pool mColliders;
            TransformAccessArray mColliderAccess;
            //NativeArray<short> mColliderAccessIndex;

            // 执行队列
            RandomList<NativeDynamicBoneRoot> mAccessList;

            bool mUpdateAccessList;
            NativeArray<short> mSystemAccessIndex;
            TransformAccessArray mSystemAccess;
            NativeArray<short> mParticleAccessIndex;
            TransformAccessArray mParticleAccess;
            //NativeArray<int> mIgnoreCollisionMatrix;
            bool mUpdateColliderAccess;
            bool mDoFindCollider;

            bool mHasJob;
            JobHandle mExecutingJob;
            List<DynamicBoneAvatar.CachedParticle> mCachedParticles;
            List<DynamicBoneColliderBase> mCachedColliders;

            public NativePool(int capacity = 128, int averageParticles = 8)
            {
                mSystemInstances = new List<NativeDynamicBoneRoot>(capacity << 1);
                mSystems = new NativeArray<DynamicBoneRootData>(capacity << 1, Allocator.Persistent);
                mParticles = new NativeArray<DynamicParticle>((capacity << 1) * averageParticles, Allocator.Persistent);
                mColliderIDs = new NativeArray<DynamicColliderID>(capacity << 2, Allocator.Persistent);

                mSystemAccessIndex = new NativeArray<short>(capacity, Allocator.Persistent);
                mSystemAccess = new TransformAccessArray(capacity);
                mParticleAccessIndex = new NativeArray<short>(capacity * averageParticles, Allocator.Persistent);
                mParticleAccess = new TransformAccessArray(capacity * averageParticles);

                mCachedParticles = new List<DynamicBoneAvatar.CachedParticle>(32);
                mCachedColliders = new List<DynamicBoneColliderBase>(8);
                mAccessList = new RandomList<NativeDynamicBoneRoot>(capacity, IsSameSystem);

                mColliderAccess = new TransformAccessArray(capacity << 1);
                //mColliderAccessIndex = new NativeArray<short>(capacity << 2, Allocator.Persistent);

                mColliders = new NativeInstance<DynamicCollider>.Pool(capacity << 1);
            }

            private bool IsSameSystem(NativeDynamicBoneRoot a, NativeDynamicBoneRoot b)
            {
                return a == b || a.mSystem == b.mSystem;
            }

            public void Dispose()
            {
                FlushJobs();
                mSystems.Dispose();
                mParticles.Dispose();
                mColliderIDs.Dispose();

                mSystemAccessIndex.Dispose();
                mSystemAccess.Dispose();
                mParticleAccessIndex.Dispose();
                mParticleAccess.Dispose();

                //mColliderAccessIndex.Dispose();
                mColliderAccess.Dispose();
                mColliders.Dispose();

                //mIgnoreCollisionMatrix.Dispose();

                for (int i = 0; i < mSystemInstances.Count; i++)
                {
                    mSystemInstances[i].mPool = null;
                    mSystemInstances[i].mNativeId = -1;
                }
                mSystemInstances.Clear();

            }

            //private void ValidateCollider(NativeInstance<DynamicCollider> obj)
            //{
            //    mDoFindCollider = true;
            //}

            public int SystemCount { get { return mSystemInstances.Count; } }

            internal DynamicBoneRootData GetSystem(int index)
            {
                return mSystems[index];
            }

            internal void MarkSystem(int index, EDynamicBoneFlags flags, bool add)
            {
                var sys = mSystems[index];
                if (add)
                    sys.flags |= flags;
                else
                    sys.flags &= ~flags;
                mSystems[index] = sys;
            }

            public DynamicParticle GetParticle(NativeDynamicBoneRoot bone, int index)
            {
                if (bone.mPool != this || bone.mNativeId == -1)
                    return default;
                FlushJobs();
                var sys = mSystems[bone.mNativeId];
                return mParticles[sys.particleIndex + index];
            }

            public DynamicCollider GetCollider(NativeDynamicBoneRoot bone, int index)
            {
                if (bone.mPool != this || bone.mNativeId == -1)
                    return default;
                FlushJobs();
                var sys = mSystems[bone.mNativeId];
                var id = mColliderIDs[sys.colliderIndex + index];
                if (id.index == -1)
                    return default;
                else
                    return mColliders.GetNativeData(id.index);
            }

            public int IndexOfInstance(DynamicBoneRoot sys)
            {
                for (int i = 0; i < mSystemInstances.Count; i++)
                {
                    var inst = mSystemInstances[i];
                    if (inst.mSystem == sys)
                        return i;
                }
                return -1;
            }

            public void FlushJobs()
            {
                if (mHasJob)
                {
                    mHasJob = false;
                    mExecutingJob.Complete();
                }
            }

            void UpdateBoneAccessList()
            {
                if (mUpdateAccessList)
                {
                    mUpdateAccessList = false;
                    NativeArrayUtil.EnsureNativeSize(ref mSystemAccessIndex, 0, mAccessList.Count);
                    NativeArrayUtil.EnsureNativeSize(ref mSystemAccess, mAccessList.Count);
                    int particleCount = 0;
                    for (int i = 0; i < mAccessList.Count; i++)
                    {
                        var inst = mAccessList[i];
                        if (inst.mSystem == null)
                            continue;
                        mSystemAccessIndex[i] = (short)inst.mNativeId;
                        if (i < mSystemAccess.length)
                            mSystemAccess[i] = inst.mSystem.transform;
                        else
                            mSystemAccess.Add(inst.mSystem.transform);
                        var sys = mSystems[inst.mNativeId];
                        NativeArrayUtil.EnsureNativeSize(ref mParticleAccessIndex, particleCount, particleCount + sys.particleCount);
                        NativeArrayUtil.EnsureNativeSize(ref mParticleAccess, particleCount + sys.particleCount);
                        for (int j = 0; j < sys.particleCount; j++)
                        {
                            var n = j + particleCount;
                            mParticleAccessIndex[n] = (short)(sys.particleIndex + j);
                            if (n < mParticleAccess.length)
                                mParticleAccess[n] = inst.mParticleTransforms[j];
                            else
                                mParticleAccess.Add(inst.mParticleTransforms[j]);
                        }
                        particleCount += sys.particleCount;
                    }
                    while (mSystemAccess.length > mAccessList.Count)
                    {
                        mSystemAccess.RemoveAtSwapBack(mAccessList.Count);
                    }
                    while (mParticleAccess.length > particleCount)
                    {
                        mParticleAccess.RemoveAtSwapBack(particleCount);
                    }
                }
            }

            void UpdateColliderAccessList()
            {
                if (mUpdateColliderAccess)
                {
                    mUpdateColliderAccess = false;
                    NativeArrayUtil.EnsureNativeSize(ref mColliderAccess, mColliders.Length);
                    for (int j = 0; j < mColliders.Length; j++)
                    {
                        var trans = mColliders[j].transform;
                        if (j < mColliderAccess.length)
                            mColliderAccess[j] = trans;
                        else
                            mColliderAccess.Add(trans);
                    }
                    while (mColliderAccess.length > mColliders.Length)
                    {
                        mColliderAccess.RemoveAtSwapBack(mColliders.Length);
                    }
                }
            }

            public void Schedule(float maxDistance, float gravityWeight, float windWeight, WindZone wind, float time, float deltaTime, float unscaledDeltaTime)
            {
#if UNITY_EDITOR
                Profiler.BeginSample("Schedule DynamicBone Jobs");
#endif
                FlushJobs();
                bool doFindCollider = mUpdateAccessList || mUpdateColliderAccess || mDoFindCollider;
                //mDoFindCollider = false;
                UpdateBoneAccessList();
                UpdateColliderAccessList();

                UpdateDynamicColliderJob updateCollider;
                updateCollider.colliders = mColliders.NativeDatas;
                //updateCollider.accessIndex = mColliderAccessIndex;
                var job = updateCollider.Schedule(mColliderAccess);

                if (doFindCollider)
                {
                    mDoFindCollider = false;
                    FindDynamicColliderJob findCollider;
                    findCollider.accessIndex = mSystemAccessIndex;
                    findCollider.colliderIds = mColliderIDs;
                    findCollider.colliders = mColliders.NativeDatas;
                    findCollider.colliderCount = mColliders.Length;
                    findCollider.dynamicSystems = mSystems;
                    job = findCollider.Schedule(mAccessList.Count, 32, job);
                }

                UpdateDynamicBoneJob updateBones;
                updateBones.gravity = Physics.gravity * gravityWeight * 0.1f;
                updateBones.dynamicSystems = mSystems;
                updateBones.maxDistance = maxDistance;
                updateBones.accessIndex = mSystemAccessIndex;
                updateBones.particles = mParticles;
                updateBones.randomSeed = time;
                updateBones.deltaTime = deltaTime;
                updateBones.unscaledDeltaTime = unscaledDeltaTime;
                updateBones.colliders = mColliders.NativeDatas;
                updateBones.colliderIds = mColliderIDs;
                if (wind != null)
                {
                    updateBones.windForce = wind.transform.forward * windWeight * wind.windMain;
                    updateBones.windFrequency = wind.windPulseFrequency;
                }
                else
                {
                    updateBones.windForce = Vector3.zero;
                    updateBones.windFrequency = 0;
                }
                job = updateBones.Schedule(mSystemAccess, job);
                ApplyDynamicBoneJob apply;
                apply.particles = mParticles;
                apply.accessIndex = mParticleAccessIndex;
                mExecutingJob = apply.Schedule(mParticleAccess, job);
                mHasJob = true;
#if UNITY_EDITOR
                Profiler.EndSample();
#endif
            }

            public NativeDynamicBoneRoot AddRootBone(DynamicBoneRoot sys)
            {
                var index = IndexOfInstance(sys);
                if (index != -1)
                    return mSystemInstances[index];
                FlushJobs();
                var inst = new NativeDynamicBoneRoot(sys);
                inst.mPool = this;
                EDynamicBoneFlags flags = 0;
                if (sys.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.X)
                    flags |= EDynamicBoneFlags.FreseAxisX;
                else if (sys.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.Y)
                    flags |= EDynamicBoneFlags.FreseAxisY;
                else if (sys.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.Z)
                    flags |= EDynamicBoneFlags.FreseAxisZ;
#if !UNSCALED_TIME
                if (sys.m_UnscaleTime)
#endif
                    flags |= EDynamicBoneFlags.UnscaledTime;
                float boneLength = 0;
                DynamicBoneAvatar.SetupParticles(mCachedParticles, sys, ref flags, ref boneLength);
                inst.mNativeId = mSystemInstances.Count;
                inst.mParticleTransforms = new Transform[mCachedParticles.Count];
                for (int i = 0; i < inst.mParticleTransforms.Length; i++)
                {
                    inst.mParticleTransforms[i] = mCachedParticles[i].transform;
                }
                mCachedColliders.Clear();
                for (int i = 0; i < sys.m_Colliders.Count; i++)
                {
                    if (sys.m_Colliders[i] != null)
                        mCachedColliders.Add(sys.m_Colliders[i]);
                }
                inst.mColliders = mCachedColliders.ToArray();
                DynamicBoneRootData sysdata;
                sysdata.layer = (byte)sys.gameObject.layer;
                sysdata.flags = flags;
                sysdata.force = sys.m_Force;
                sysdata.radius = boneLength;
                sysdata.orientation = sys.transform.rotation;
                sysdata.particleCount = (short)mCachedParticles.Count;
                sysdata.particleIndex = (short)mParticleCount;
                sysdata.prevPosition = sys.transform.position;
                sysdata.colliderIndex = (short)mColliderIdCount;
                sysdata.colliderCount = (short)mCachedColliders.Count;
                sysdata.time = 0;
                sysdata.forceWeight = sys.m_WorldForce;
                sysdata.updateRatio = 1f / Mathf.Max(30f, sys.m_UpdateRate);
                NativeArrayUtil.EnsureNativeSize(ref mSystems, mSystemInstances.Count, mSystemInstances.Count + 1);
                NativeArrayUtil.EnsureNativeSize(ref mParticles, mParticleCount, mParticleCount + mCachedParticles.Count);
                NativeArrayUtil.EnsureNativeSize(ref mColliderIDs, mColliderIdCount, mColliderIdCount + mCachedColliders.Count);
                mSystems[inst.mNativeId] = sysdata;
                mSystemInstances.Add(inst);
                mParticleCount += sysdata.particleCount;
                mColliderIdCount += sysdata.colliderCount;
                for (int i = 0; i < sysdata.particleCount; i++)
                {
                    mParticles[i + sysdata.particleIndex] = mCachedParticles[i].data;
                }
                for (int i = 0; i < sysdata.colliderCount; i++)
                {
                    DynamicColliderID cid;
                    cid.id = mCachedColliders[i].InstanceID;
                    cid.index = -1;
                    mColliderIDs[i + sysdata.colliderIndex] = cid;
                }
                return inst;
            }

            public void RemoveRootBone(NativeDynamicBoneRoot sys)
            {
                var index = sys.mNativeId;
                if (sys.mPool != this || index < 0 || index >= mSystemInstances.Count)
                    return;
                FlushJobs();
                sys.mNativeId = -1;
                sys.mPool = null;
                var sysdata = mSystems[index];
                mSystemInstances.RemoveAt(index);
                for (int i = index; i < mSystemInstances.Count; i++)
                {
                    mSystemInstances[i].mNativeId = i;
                    var data = mSystems[i + 1];
                    data.particleIndex -= sysdata.particleCount;
                    data.colliderIndex -= sysdata.colliderCount;
                    mSystems[i] = data;
                }
                mParticleCount -= sysdata.particleCount;
                for (int i = sysdata.particleIndex; i < mParticleCount; i++)
                {
                    mParticles[i] = mParticles[i + sysdata.particleCount];
                }
                mColliderIdCount -= sysdata.colliderCount;
                for (int i = sysdata.colliderIndex; i < mColliderIdCount; i++)
                {
                    mColliderIDs[i] = mColliderIDs[i + sysdata.colliderCount];
                }
                // 确保更新正确, 清空执行队列后重新计算
                mAccessList.Remove(sys);
                mUpdateAccessList = true;
            }

            public void AddUpdateData(NativeDynamicBoneRoot sys)
            {
                if (!mAccessList.Contains(sys))
                {
                    mAccessList.Add(sys);
                    mUpdateAccessList = true;
                }
            }

            public void RemoveUpdateData(NativeDynamicBoneRoot sys)
            {
                if (mAccessList.Remove(sys))
                {
                    mUpdateAccessList = true;
                }
            }

            public NativeInstance<DynamicCollider> AddCollider(DynamicBoneColliderBase collider)
            {
                FlushJobs();
                mUpdateColliderAccess = true;
                return mColliders.AddInstance(collider);
            }

            public void RemoveCollider(NativeInstance<DynamicCollider> inst)
            {
                FlushJobs();
                mUpdateColliderAccess |= mColliders.RemoveInstance(inst);
            }

            public void MarkRootDirty(NativeDynamicBoneRoot sys, bool fullUpdate = false)
            {
                if (sys.mNativeId == -1 || sys.mSystem == null)
                    return;
                FlushJobs();
                var data = mSystems[sys.mNativeId];
                data.force = sys.mSystem.m_Force;
                data.forceWeight = sys.mSystem.m_WorldForce;
                if (fullUpdate)
                {
                    data.layer = (byte)sys.mSystem.gameObject.layer;
                    EDynamicBoneFlags flags = 0;
                    if (sys.mSystem.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.X)
                        flags |= EDynamicBoneFlags.FreseAxisX;
                    else if (sys.mSystem.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.Y)
                        flags |= EDynamicBoneFlags.FreseAxisY;
                    else if (sys.mSystem.m_FreezeAxis == DynamicBoneRoot.FreezeAxis.Z)
                        flags |= EDynamicBoneFlags.FreseAxisZ;
#if !UNSCALED_TIME
                if (sys.mSystem.m_UnscaleTime)
#endif
                    flags |= EDynamicBoneFlags.UnscaledTime;
                    if (sys.mSystem.SimulateSpace == Space.Self)
                        flags |= EDynamicBoneFlags.LocalSimulate;
                    //if ((data.flags & (byte)EDynamicBoneFlags.FixedOrientation) != 0)
                    //    flags |= EDynamicBoneFlags.FixedOrientation;
                    data.flags = flags;
                    // update colliders
                    mCachedColliders.Clear();
                    for (int i = 0; i < sys.mSystem.m_Colliders.Count; i++)
                    {
                        if (sys.mSystem.m_Colliders[i] != null)
                            mCachedColliders.Add(sys.mSystem.m_Colliders[i]);
                    }
                    sys.mColliders = mCachedColliders.ToArray();
                    var dlen = mCachedColliders.Count - data.colliderCount;
                    if (dlen > 0)
                    {
                        var n = data.colliderIndex + data.colliderCount + dlen;
                        NativeArrayUtil.EnsureNativeSize(ref mColliderIDs, mColliderIdCount, mColliderIdCount + dlen);
                        mColliderIdCount += dlen;
                        for (int i = mColliderIdCount - 1; i >= n; i--)
                        {
                            mColliderIDs[i] = mColliderIDs[i - dlen];
                        }
                    }
                    else if (dlen < 0)
                    {
                        mColliderIdCount += dlen;
                        for (int i = data.colliderIndex + data.colliderCount + dlen; i < mColliderIdCount; i++)
                        {
                            mColliderIDs[i] = mColliderIDs[i - dlen];
                        }
                    }
                    if (dlen != 0)
                    {
                        for (int i = sys.mNativeId + 1; i < mSystemInstances.Count; i++)
                        {
                            var next = mSystems[i];
                            next.colliderIndex += (short)dlen;
                            mSystems[i] = next;
                        }
                    }
                    data.colliderCount = (short)mCachedColliders.Count;
                    for (int i = 0; i < data.colliderCount; i++)
                    {
                        var col = mColliderIDs[i + data.colliderIndex];
                        if (col.id != sys.mColliders[i].InstanceID)
                        {
                            col.id = sys.mColliders[i].InstanceID;
                            col.index = -1;
                            mColliderIDs[i + data.colliderIndex] = col;
                            mDoFindCollider = true;
                        }
                    }

                    // update particles
                    float boneLength = 0;
                    mCachedParticles.Clear();
                    var toWorld = sys.mSystem.transform.localToWorldMatrix;
                    for (int i = 0; i < sys.ParticleCount; i++)
                    {
                        var p = new DynamicBoneAvatar.CachedParticle(sys.mParticleTransforms[i]);
                        p.data = mParticles[data.particleIndex + i];
                        mCachedParticles.Add(p);
                        var p0 = p.data.parentIndex == -1 ? null : mCachedParticles[p.data.parentIndex];
                        if (p0 != null)
                        {
                            p.boneLength = p0.boneLength + toWorld.MultiplyVector(p.data.initLocalPosition - p0.data.initLocalPosition).magnitude;
                            boneLength = Mathf.Max(p.boneLength, boneLength);
                        }
                    }
                    for (int i = 0; i < mCachedParticles.Count; i++)
                    {
                        mCachedParticles[i].UpdateParameters(sys.mSystem, boneLength);
                        mParticles[data.particleIndex + i] = mCachedParticles[i].data;
                    }
                }
                else
                {
                    if (sys.mSystem.SimulateSpace == Space.Self)
                        data.flags |= EDynamicBoneFlags.LocalSimulate;
                    else
                        data.flags &= ~EDynamicBoneFlags.LocalSimulate;
                }
                mSystems[sys.mNativeId] = data;
            }

        }


    }
}